#include <zephyr/kernel.h>
#include <zephyr/device.h>
#include <zephyr/devicetree.h>
#include <zephyr/drivers/gpio.h>
#include <zephyr/drivers/pwm.h>
#include <zephyr/logging/log.h>
#include "include/relay.h"
#include "include/servo.h"
#include "include/stepmotor.h"
#include "include/common.h"

LOG_MODULE_REGISTER(stepmotor);

#define  STEPMOTOR_FORWARD   GPIO_OUTPUT_HIGH
#define  STEPMOTOR_BACKWARD   GPIO_OUTPUT_LOW

extern const struct gpio_dt_spec *stepmotor_dir, *stepmotor_limit;
extern const struct pwm_dt_spec *stepmotor_pwm ;

K_TIMER_DEFINE(stepmotor_timer, stepmotor_timer_function, NULL);
Stepmotor_Sta  Stepmotor_Sta_g;


int Stepmotor_init(void)
{
    if (!device_is_ready(stepmotor_pwm -> dev) || !device_is_ready(stepmotor_dir -> port) || !device_is_ready(stepmotor_limit -> port) ) {
        /* Not ready, do not use */
        LOG_ERR("stepmotor device is not ready!");
        return -ENODEV;
	}
    else{
        gpio_pin_configure_dt(stepmotor_limit, GPIO_INPUT);
        gpio_pin_configure_dt(stepmotor_dir, STEPMOTOR_BACKWARD); 
        pwm_set_pulse_dt(stepmotor_pwm, STEPMOTOR_PULSE);
        while(gpio_pin_get_dt(stepmotor_limit) != 0){
        //  printk("waiting stepmotor limit\n");
          k_sleep(K_MSEC(100));//防止看门狗操作
        }
        LOG_INF("stepmotor limit arrived");
        pwm_set_pulse_dt(stepmotor_pwm, 0);
        Stepmotor_Sta_g.dist = 0;
        
        return 0;
    }
}

/*默认arr
	42电机步距角为1.8°/脉冲    不细分
	360/1.8=100  个脉冲电机转一圈
	一个脉冲周期设为0.625ms 
	则0.065s转1圈 1秒转16圈
	丝杆一圈4毫米，则一秒6.4cm
*/

int Stepmotor_ctr_API(Stepmotor_Ctr Stepmotor_Ctr_val) 
{
    if (!device_is_ready(stepmotor_pwm -> dev) || !device_is_ready(stepmotor_dir -> port) || !device_is_ready(stepmotor_limit -> port) ) {
        /* Not ready, do not use */
        LOG_ERR("stepmotor device is not ready!");
        return -ENODEV;
	}
    else{
        float time;

        if(Stepmotor_Ctr_val.dist  > stepping_motor_maxdist) {
            Stepmotor_Ctr_val.dist  = stepping_motor_maxdist;
        }
        if(Stepmotor_Ctr_val.dist < 0){
            Stepmotor_Ctr_val.dist = 0;
        }

        if((Stepmotor_Ctr_val.dist != Stepmotor_Sta_g.dist) ){
            if(Stepmotor_Ctr_val.dist > Stepmotor_Sta_g.dist) {
			    gpio_pin_configure_dt(stepmotor_dir, STEPMOTOR_FORWARD); 
		        time = (Stepmotor_Ctr_val.dist - Stepmotor_Sta_g.dist) / 6.4;
		    }
            else{
        	    gpio_pin_configure_dt(stepmotor_dir, STEPMOTOR_BACKWARD); 
                time = (Stepmotor_Sta_g.dist - Stepmotor_Ctr_val.dist) / 6.4; 
            }

            pwm_set_pulse_dt(stepmotor_pwm, STEPMOTOR_PULSE);

            k_timer_start(&stepmotor_timer, K_MSEC(time * 1000), K_FOREVER);
            Stepmotor_Sta_g.dist = Stepmotor_Ctr_val.dist;
        }
    }  
    return 0;
}

void stepmotor_timer_function(struct k_timer *timer_id){
    pwm_set_pulse_dt(stepmotor_pwm, 0);
    k_timer_stop(&stepmotor_timer);
}